1. Field of the Invention
The present invention relates to a system control device, especially to a control device for instructing a robot manipulator to track an object trajectory, and more specifically to an adaptive robust control device for guaranteeing an excellent trajectory tracking capability even if a physical parameter characterizing the dynamic behavior is unknown or even if a random disturbance such as a sensor noise is applied.
2. Description of the Related Art
In the trajectory tracking control of an n-link-structured robot manipulator, the dynamic characteristics of a robot manipulator is described using a mathematical model of the following equation in, for example, document 1 titled "Introduction to Robotics: Mechanics and Control," by J. J. Craig published in 1989 by Addison-Wesley, EQU M(q)q+C(q,q)q+G(q)=.tau..sub.1 ( 1)
where q indicates an n-dimensional joint variable, and q indicates the time differential (d/dt) of q. In equation (1) as a model, M(q) indicates an inertia matrix. C(q, q)q is an n-dimensional vector and describes the influence of the Coriolis force and the centrifugal force. G(q) is an n-dimensional vector representing the influence of gravity. .tau. is an n-dimensional vector representing the input torque applied from the actuators. The purpose of the trajectory tracking control is to generate the control torque .tau. that allows the trajectory q(t) of the robot manipulator to follow a desired trajectory q.sub.d (t). PA1 where q, M(q), C(q, q), and G(q) indicate vectors and matrices in equation (1), and q.sub.d indicates an object trajectory. K.sub.p and K.sub.v indicate constant matrices prescribing the servo-compensation gain. In equation (2), the first and second terms of the right-hand-side are input units for linearization while the remaining units perform servo-compensation. The trajectory tracking error e=q-q.sub.d follows the linear equation expressed below by inputting equation (2) representing the control torque to the robot system expressed by equation (1). EQU e(t)+K.sub.v e(t)+K.sub.p e(t)=0, (3) PA1 where S.sup.T (q, q) indicates the transpose of S(q, q). As shown in equation (1), the dynamic characteristic model is a non-linear equation if it relates to a joint angle, but is a linear equation if it relates to a parameter obtained by appropriately combining physical parameters (center of mass, center of inertia, etc.). The combined vector is expressed as .sigma..epsilon.R.sup.m. PA1 where .sigma. indicates an estimated value of .sigma.. PA1 where q.sub.r indicates a variable defined by equation (6). The first term of the right-hand-side in equation (9) indicates the feed-forward component using an estimated value. Assuming that the estimated values of the coefficient matrices M(q), C(q, q), and G(q) are M(q), C(q, q), and G(q) respectively in equation (1), the first term of the right-hand-side in equation (9) is expressed as follows, EQU Y(q,q,q.sub.r,q.sub.r).sigma.(t)=M(q)q.sub.r (t)+C(q,q)q.sub.r (t)+G(q).(10 )
The typical conventional technologies relating to the robot trajectory tracking control are listed below. The first related art is the linear feedback control device. This control device is described in the above described document 1. In this device, an independent position and speed feedback loop is designed for each joint. The object trajectory of each joint is given as a reference input for the control.
This control device is effective for a robot which is provided with a reducer having a large reduction ratio between the actuator and the arm in the mechanism of the robot, and acts at a relatively low speed. However, it is difficult to apply this control device when an operation should be performed at a high speed with high-precision or to a robot having a direct drive mechanism widely applicable to the fields other than the FA (Factory Automation).
The second related art is the computed torque control device. This control device is also described in document 1. It comprises a linear unit and a servo-compensation unit, and its input torque is designed as expressed by the following equation. EQU .tau.(t)=C(q,q)q+G(q)+M(q){q.sub.d +K.sub.v (q.sub.d -q)+K.sub.p (q.sub.d -q)} (2)
Therefore, a desired servo-compensation characteristic can be obtained by appropriately selecting matrices K.sub.p and K.sub.v.
The computation torque control device is based on the premise that the parameters in the coefficient matrix M (q), C(q, q) G(q) in equation (1) are completely known. However, since the robot is a structure made of a large number of units, it is often troublesome to correctly identify these parameters. The adaptive control device described below has been developed to track an object trajectory with asymptotic stability even in the above described case.
The third related art is the adaptive control device according to document 2 "Applied Nonlinear Control" by J. E. Slotine and W. Li, published in 1991 by Prentice-Hall.
Normally, equation (1) as a dynamic characteristic model of a manipulator can be expressed as follows. ##EQU1## where matrix S(q, q) satisfies the following equation (5). EQU S(q,q)+S.sup.T (q,q)=0 (5)
The following variable is introduced for a tracking error to obtain a control input which guarantees an asymptotic stability of the control system described by the above dynamic characteristic model ##EQU2## where q.sub.d indicates an object trajectory as in equation (2), and .LAMBDA. indicates an optional positive definite n.times.n matrix. The following equation is used as a candidate for a Lyapuov functional in a trajectory tracking control system. ##EQU3## where .GAMMA. indicates an optional positive definite mxm matrix, and .sigma. indicates an estimation error expressed as follows. EQU .sigma.(t)=.sigma.(t)-.sigma. (8)
To guarantee the asymptotic stability, the control input should be set with V(t)&lt;0. However, the adaptive control device generates the following input by considering the relations expressed in equations (4) and (5), and the linearity of the dynamic characteristic relating to the system parameter. EQU .tau.(t)=Y(q,q,q.sub.r,q.sub.r).sigma.(t)-K.sub.D s(t) (9)
Matrices M(q), C(q, q), and G(q) are expressed by the estimated value .sigma. of a system parameter, but the estimated value is updated by the following adaptation law. ##EQU4## where K.sub.D in the second term of the right-hand-side indicates an optional positive definite n.times.n matrix.
FIG. 1 is a block diagram showing the configuration of the conventional adaptive control device.
In FIG. 1, the adaptive control device comprises a trajectory tracking error operation unit 51 for obtaining a trajectory tracking error based on the actual trajectory and an object trajectory of a robot 50; a parameter updating unit 52 for updating the estimated value of a system parameter according to equation (11) as a parameter adaptation law; an adaptive linearization input generation unit 53 for generating a partial input in equation (10) as a partial input for adaptive linearization of a system; a feedback gain multiplication unit 54 for generating a partial input in the second term of the right-hand-side in equation (9); and a subtractor 55 for subtracting an output from the feedback gain multiplication unit 54 from the output from the adaptive linearization input generation unit 53 and providing the subtraction result for the robot 50 as an input.
As shown in equations (9) through (11), an adaptation law on a system parameter is incorporated into the adaptive control device to correspond to an incorrect system parameter. Based on the estimated value obtained from the adaptation law, the adaptive control device performs a nonlinear compensation, and simultaneously performs an error PD feedback. Thus, the adaptive control device realizes a trajectory tracking with asymptotic stability even if the system parameter is not correctly estimated.
If the following equation (12) is substituted into equation (9), the control input is expressed as follows in equation (13). EQU K.sub.D =M(q).LAMBDA. (12) EQU .tau.(t)=C(q,q)q.sub.r +G(q)+M(q){q.sub.d +2.LAMBDA.(q.sub.d -q)+.LAMBDA..sup.2 (q.sub.d -q)} (13)
Thus, the input is similar to that with the computed torque control device. Since the positive definiteness of M(q) is not guaranteed, a system parameter to be estimated is extracted from the relation expressed by the following equation (14) to select K.sub.D as shown in the equation (12) and guarantee the asymptotic stability. Then, the adaptation law on them should be amended as shown by the following equation (15). EQU Y.sub.m (q,q,q.sub.r,q.sub.r) .sigma.(t)=M(q)(q.sub.r (t)-.LAMBDA.s(t))+C(q,q)q.sub.r (t)+G(q) (14) ##EQU5##
At this time, the generated input is expressed by equation (13), but can also be expressed as follows. EQU .tau.(t)=Y.sub.m (q,q,q.sub.r,q.sub.r) .sigma.(t) (16)
As shown in equation (13), the adaptive control device can also generate an input similar to that generated by the computed torque control device. However, unlike the computed torque control device, it cannot freely provide a desired servo-compensation characteristic, or cannot appropriately remove the influence of random disturbance such as irregular external forces or noise from the environment, etc.
A robot is a structure made of a number of materials, and it is troublesome to correctly estimate the coefficient matrices M(q), C(q, q), and G(q) in equation (1) as a dynamic characteristic model. Because of the influence of random disturbance such as noise, a robot often fails to obtain a desired performance.
On the other hand, most conventional technologies are based on the premise that the coefficient matrix of a dynamic characteristic model is correctly given. A parameter adaptation law may be incorporated into some conventional technologies in consideration of the error in a model or of an influence of an external input whose size is defined. However, very few conventional technologies have successfully developed a robust control device with a modelling error and the influence of random disturbance such as noise taken into account. Actually, there is only the technology described in the following document 3 by the Inventor, that is, "A robust motion control of manipulators with parametric uncertainties and random disturbances" in Proc. of 34th IEEE Conf. Decision and Control, page 1609 through 1611, 1995.